#include "WPILib.h"
#include "PandroidSettings.h"
#include "DriverConsole.h"
#include "MotorWrapper.h"
#include "Math.h"

class TurnPID : public PIDOutput {
public:
	TurnPID(MotorWrapper *motorFL, MotorWrapper *motorFR, MotorWrapper *motorBL, MotorWrapper *motorBR) {
		_motorFL = motorFL;
		_motorFR = motorFR;
		_motorBL = motorBL;
		_motorBR = motorBR;
	}
	
	void SetMotors() {
		
	}

	void PIDWrite(float output) {
		_motorFL->SetSetpoint(Limit(output));
		_motorBL->SetSetpoint(Limit(output));
		_motorFR->SetSetpoint(-Limit(output));
		_motorBR->SetSetpoint(-Limit(output));
	}
	
	float Limit(float num) {
		if (num > 1)
			num = 1;
		else if (num < -1) 
			num = -1;
		return num;
	}
private:
	MotorWrapper *_motorFL;
	MotorWrapper *_motorFR;
	MotorWrapper *_motorBL;
	MotorWrapper *_motorBR;
};

class LinearPID : public PIDOutput {
public:
	LinearPID(MotorWrapper *motorFL, MotorWrapper *motorFR, MotorWrapper *motorBL, MotorWrapper *motorBR) {
		_motorFL = motorFL;
		_motorFR = motorFR;
		_motorBL = motorBL;
		_motorBR = motorBR;
	}

	void PIDWrite(float output) {
		_motorFL->SetSetpoint(Limit(output));
		_motorBL->SetSetpoint(Limit(output));
		_motorFR->SetSetpoint(Limit(output));
		_motorBR->SetSetpoint(Limit(output));
	}
	
	float Limit(float num) {
		if (num > 1)
			num = 1;
		else if (num < -1) 
			num = -1;
		return num;
	}
private:
	MotorWrapper *_motorFL;
	MotorWrapper *_motorFR;
	MotorWrapper *_motorBL;
	MotorWrapper *_motorBR;
};

class AllWheelEncoder : public PIDSource {
public:
	AllWheelEncoder(MotorWrapper *motorFL, MotorWrapper *motorFR, MotorWrapper *motorBL, MotorWrapper *motorBR) {
		_motorFL = motorFL;
		_motorFR = motorFR;
		_motorBL = motorBL;
		_motorBR = motorBR;
	}

	double PIDGet() {
		float sum = _motorFL->GetState() + _motorFR->GetState() + _motorBL->GetState() + _motorBR->GetState();
		return sum / 4;
	}
	
private:
	MotorWrapper *_motorFL;
	MotorWrapper *_motorFR;
	MotorWrapper *_motorBL;
	MotorWrapper *_motorBR;
};

class DriveSystem {
private:
	DriveSystem(DriverConsole *console, NetworkTable *smartDash);
public:
	typedef enum {tank = 0, arcade = 1} DriveMode;
	static DriveSystem* GetInstance(DriverConsole* console, NetworkTable *smartDash);
	virtual ~DriveSystem();
	void Teleop(DriveMode mode);
	void MoveForward(float dist);
	void Turn(float angle);
	float Limit(float num);
	float SignSquare(float num);
	void SetMotorSpeeds(float speed1, float speed2, float speed3, float speed4);
	void UpdateFromDash();
	void DashPost();
	float Round(float num, int numPlaces);
private:
	//I guess this thing is a singleton, so we might as well treat it like one
	static DriveSystem *_thisInstance;
	
	Gyro *_gyro;
	AllWheelEncoder *_awenc;
	PIDOutput *_turnPID;
	PIDOutput *_linPID;
	PIDController *_turnPIDController;
	PIDController *_linPIDController;
	DriverConsole *_driverConsole;
	NetworkTable *_smartDash;
	MotorWrapper *_motorFL;
	MotorWrapper *_motorFR;
	MotorWrapper *_motorBL;
	MotorWrapper *_motorBR;
};
